﻿// test for configurations
// created on 1st June 2021
// @author ZHUOXU WHU
// for INS/GNSS Loose Couple Algorithm

#include <iostream>
#include "config.h"
#include "decode_imr.h"
#include "imr_converter.h"
#include "PureINS.h"
#include "gnss.h"
#include "loose_coupled.h"
#include "initial_alignment.h"

using namespace std;

int main(int argc, char** argv) {

	if (argc != 5) {
		fprintf(stdout, "ERROR argc != 5. USAGE: ./NavNewton.exe gnss_path imr_path config_path out_res_path\n");
		fflush(stdout);
		exit(EXIT_FAILURE);
	}
	fprintf(stdout, "NavNewton NORMAL START\n");
	fflush(stdout);
	//string gnss_path = R"(.\test_data\RTK_E_湖面.PosT)";
	//string imr_filepath = R"(.\test_data\IMR_E_湖面.imr)";
	//ofstream outresfile(R"(.\test_data\LCres_湖面.txt)");
	string gnss_path = argv[1];
	string imr_filepath = argv[2];
	char configPath[2048];
	char outresPath[2048];
	strcpy(configPath, argv[3]);
	strcpy(outresPath, argv[4]);
	ofstream outresfile(outresPath);
	fprintf(stdout, "gnss_path: %s\nimr_path: %s\nconfig_path: %s\nout_res_path: %s\n", gnss_path.c_str(), imr_filepath.c_str(), configPath, outresPath);

	// config 读取
	CConfig config(configPath);
	prcopt conf = config.GetConf();

	//零偏看情况输出
	//string bias_filepath = R"(D:\2021study\IntNav\data\实习数据\松组合结果\备份\6_23\bias.txt)";

	 //0 获得RTK定位结果
	GNSS _gnss;
	_gnss.readFile(gnss_path);
	int epoch_index=1;//GNSS结果历元序号，先置1
	GNSSEpoch _gnss_epoch;

	//松组合准备
	Vector3d g_bias;g_bias<< 0.0, 0.0, 0.0;//陀螺零偏（先置0）
	Vector3d a_bias;a_bias << 0.0, 0.0, 0.0;//加计零偏（先置0）
	Vector3d imu_pos;
	Vector3d imu_vel; 
	Vector3d imu_eul;
	Vector3d f_bi_b;
	Vector3d w_bi_b;

	Vector3d lever_arm; lever_arm << conf.lever_[0], conf.lever_[1], conf.lever_[2];//前右下杆臂值
	KF _LC_KF(conf);//构造函数：包括初始化P阵和Q的系数阵S

	// 1 从IMR文件中解析出INS RAW输出（直接读出的IMR输出的XYZ：右前上，ENU）===============
	FILE* imr_fp;//imr file temp
	errno_t err = fopen_s(&imr_fp, imr_filepath.c_str(), "rb");//以二进制的方式打开imr文件，若打开成功返回0
	if (err != 0) {
		cerr << "Cannot open imr bin file.\n";
		exit(-1);
	}


	// 2 INS初始对准==============================

	// 计算当地正常重力
	//vector<double> blh_init = WGS84_XYZ2BLH(vector<double>{conf.init_pos_[0], conf.init_pos_[1], conf.init_pos_[2]});
	vector<double> blh_init = vector<double>{ conf.init_pos_[0], conf.init_pos_[1], conf.init_pos_[2] };
	double B_init = Deg2Rad(blh_init[0]);
	double L_init = Deg2Rad(blh_init[1]);
	double H_init = blh_init[2];
	double g_init;
	if (conf.gravity_ < 0) {
		g_init = Cal_Gravity(B_init, H_init);
	}
	else {
		g_init = conf.gravity_;
	}
	IMRData imr_data(imr_fp);

	//将INIT_ALIGN_EPOCH个历元的IMR数据装载用于初始对准
	InitAlign init_align;
	int init_align_epoch = conf.align_time_*conf.sampling_rate_;
	MatrixXd raw_accel_for_init(init_align_epoch, 3);
	MatrixXd raw_gyro_for_init(init_align_epoch, 3);

	for (int i = 0; i < init_align_epoch; i++)
	{
		imr_data.getIMURawData(imr_fp);//直接读出的IMR输出的XYZ：右前上，ENU
		raw_accel_for_init(i, 0) = imr_data._imu_raw_data._ay;
		raw_accel_for_init(i, 1) = imr_data._imu_raw_data._ax;
		raw_accel_for_init(i, 2) = -imr_data._imu_raw_data._az;
		raw_gyro_for_init(i, 0) = Deg2Rad(imr_data._imu_raw_data._gy);
		raw_gyro_for_init(i, 1) = Deg2Rad(imr_data._imu_raw_data._gx);
		raw_gyro_for_init(i, 2) = Deg2Rad( -imr_data._imu_raw_data._gz);
	}
	//outfile << "#==========================\n";
	init_align.set(raw_accel_for_init, raw_gyro_for_init, g_init, B_init);

	Matrix3d rot_mat = init_align.initAlign();
	Vector3d rpy_euler = RotationMatrixToEuler(rot_mat);//得到初始姿态角 raw pitch yaw
	Quaterniond q_init = EulerToQuaternion(rpy_euler);// 四元数

	// 3 INS解算和松组合解算，其中坐标系转到前右下，NED====================================

	// 初始化第一个历元的姿态、速度、位置
	INS_Result ins_res_now;
	INS_Result ins_res_bef;

	IMR_Data_LJN imr_now;
	IMR_Data_LJN imr_bef;
	
	ins_res_now.attitude.Q.Q0 = q_init.w();
	ins_res_now.attitude.Q.Q1 = q_init.x();
	ins_res_now.attitude.Q.Q2 = q_init.y();
	ins_res_now.attitude.Q.Q3 = q_init.z();
	ins_res_now.attitude.E.Roll = rpy_euler(0);
	ins_res_now.attitude.E.Pitch = rpy_euler(1);
	ins_res_now.attitude.E.Yaw = rpy_euler(2);

	//目前仅支持静态对准
	ins_res_now.velocity.VN = 0.0;
	ins_res_now.velocity.VE = 0.0;
	ins_res_now.velocity.VD = 0.0;

	ins_res_now.position.B = B_init;
	ins_res_now.position.L = L_init;
	ins_res_now.position.H = H_init;

	// 转换成增量
	double SAMPLE_INTERVAL = 1.0 / conf.sampling_rate_;
	imr_now._time = imr_data._imu_raw_data._time_sow;
	imr_now._accel_x = SAMPLE_INTERVAL * imr_data._imu_raw_data._ay;
	imr_now._accel_y = SAMPLE_INTERVAL * imr_data._imu_raw_data._ax;
	imr_now._accel_z = SAMPLE_INTERVAL * (-imr_data._imu_raw_data._az);
	imr_now._gyro_x = SAMPLE_INTERVAL * Deg2Rad(imr_data._imu_raw_data._gy);
	imr_now._gyro_y = SAMPLE_INTERVAL * Deg2Rad(imr_data._imu_raw_data._gx);
	imr_now._gyro_z = SAMPLE_INTERVAL * Deg2Rad(-imr_data._imu_raw_data._gz);

	/*寻找当前惯导结果后的下一个GNSS结果位置*/
	for (int i = 0; i < _gnss._data_size; i++)
	{
		double _gnss_t = _gnss._GNSS_result[i][1];
		if ((imr_now._time - _gnss_t) < 0 && (imr_now._time - _gnss_t) > -1.0)
		{
			epoch_index = i + 1;//接下来的GNSS的历元序号
			break;
		}
	}

	// INS解算与松组合循环
	//输出零偏：
	//ofstream outbiasfile(bias_filepath);
	PureINS pure_ins;
	while (imr_fp)
	{
		// before = now
		ins_res_bef = ins_res_now;

		imr_bef = imr_now;

		imr_data.getIMURawData(imr_fp);

		// 转换成增量
		imr_now._time = imr_data._imu_raw_data._time_sow;
		imr_now._accel_x = SAMPLE_INTERVAL * imr_data._imu_raw_data._ay;
		imr_now._accel_y = SAMPLE_INTERVAL * imr_data._imu_raw_data._ax;
		imr_now._accel_z = SAMPLE_INTERVAL * (-imr_data._imu_raw_data._az);
		imr_now._gyro_x = SAMPLE_INTERVAL * Deg2Rad(imr_data._imu_raw_data._gy);
		imr_now._gyro_y = SAMPLE_INTERVAL * Deg2Rad(imr_data._imu_raw_data._gx);
		imr_now._gyro_z = SAMPLE_INTERVAL * Deg2Rad(-imr_data._imu_raw_data._gz);

		//反馈调节零偏
		imr_now._accel_x -= SAMPLE_INTERVAL * a_bias(0);
		imr_now._accel_y -= SAMPLE_INTERVAL * a_bias(1);
		imr_now._accel_z -= SAMPLE_INTERVAL * a_bias(2);
		imr_now._gyro_x -= SAMPLE_INTERVAL * g_bias(0);
		imr_now._gyro_y -= SAMPLE_INTERVAL * g_bias(1);
		imr_now._gyro_z -= SAMPLE_INTERVAL * g_bias(2);

		// 速度、姿态、位置依次更新
		pure_ins.Velocity_Updating(&imr_bef, &imr_now, &ins_res_bef, &ins_res_now);
		pure_ins.Attitude_Updating(&imr_bef, &imr_now, &ins_res_bef, &ins_res_now);
		pure_ins.Postion_Updating(&imr_bef, &imr_now, &ins_res_bef, &ins_res_now);

		//predict: P阵随着机械编排进行预测转化
		imu_pos << ins_res_bef.position.B, ins_res_bef.position.L, ins_res_bef.position.H;
		imu_vel << ins_res_bef.velocity.VN, ins_res_bef.velocity.VE, ins_res_bef.velocity.VD;
		imu_eul << ins_res_bef.attitude.E.Roll, ins_res_bef.attitude.E.Pitch, ins_res_bef.attitude.E.Yaw;
		f_bi_b << imr_bef._accel_x / SAMPLE_INTERVAL,
			imr_bef._accel_y / SAMPLE_INTERVAL,
			imr_bef._accel_z / SAMPLE_INTERVAL;
		double dT = imr_now._time - imr_bef._time;
		_LC_KF.processPredict(imu_pos, imu_vel, imu_eul, f_bi_b, dT);

		//update: 在GNSS存在值的时间范围内,当有GNSS值时进行测量更新*/
		int week = _gnss._GNSS_result[epoch_index - 1][0];
		if (fabs(imr_now._time - _gnss._GNSS_result[epoch_index - 1][1]) < 0.005&&epoch_index <= _gnss._data_size)
		{
			/*先用机械编排新值进行杆臂改正*/
			_gnss.getGNSSres(epoch_index, _gnss_epoch);
			epoch_index += 1;
			imu_pos << ins_res_now.position.B, ins_res_now.position.L, ins_res_now.position.H;
			imu_vel << ins_res_now.velocity.VN, ins_res_now.velocity.VE, ins_res_now.velocity.VD;
			imu_eul << ins_res_now.attitude.E.Roll, ins_res_now.attitude.E.Pitch, ins_res_now.attitude.E.Yaw;
			w_bi_b << imr_now._gyro_x / SAMPLE_INTERVAL,
				imr_now._gyro_y / SAMPLE_INTERVAL,
				imr_now._gyro_z / SAMPLE_INTERVAL;
			//杆臂改正
			correctLeverArm(_gnss_epoch._gnss_pos, _gnss_epoch._gnss_vel, imu_pos, imu_vel, imu_eul, w_bi_b, lever_arm);
			/*再用机械编排新值对其进行更新*/
			_LC_KF.processUpdate(imu_pos, imu_vel, imu_eul, g_bias, a_bias, _gnss_epoch._gnss_pos, _gnss_epoch._gnss_vel, _gnss_epoch._Dxx, _gnss_epoch._Dvv);
			//结果反赋给机械编排的结果结构体
			ins_res_now.position.B = imu_pos(0), ins_res_now.position.L = imu_pos(1), ins_res_now.position.H = imu_pos(2);
			ins_res_now.velocity.VN = imu_vel(0), ins_res_now.velocity.VE = imu_vel(1), ins_res_now.velocity.VD = imu_vel(2);
			ins_res_now.attitude.E.Roll = imu_eul(0), ins_res_now.attitude.E.Pitch = imu_eul(1), ins_res_now.attitude.E.Yaw = imu_eul(2);
			Vector4d _q;
			calQbyEul(_q, imu_eul);
			ins_res_now.attitude.Q.Q0 = _q(0); ins_res_now.attitude.Q.Q1 = _q(1); ins_res_now.attitude.Q.Q2 = _q(2); ins_res_now.attitude.Q.Q3 = _q(3);
		}
		//结果文件输出
		//outresfile << fixed << setprecision(10) << setw(10) << imr_data._imu_raw_data._time_sow << "  " << Rad2Deg(ins_res_now.position.B) << "  " << Rad2Deg(ins_res_now.position.L) << "  " << ins_res_now.position.H << "  "
		//	<< fixed << setprecision(10) << setw(10) << ins_res_now.velocity.VN << "  " << ins_res_now.velocity.VE << "  " << ins_res_now.velocity.VD << "  "
		//	<< fixed << setprecision(10) << setw(10) << Rad2Deg(ins_res_now.attitude.E.Roll) << "  " << Rad2Deg(ins_res_now.attitude.E.Pitch) << "  " << Rad2Deg(ins_res_now.attitude.E.Yaw) << "\n";
		char buffer[2048];
		sprintf(buffer, "%4d%14.4lf%18.10lf%18.10lf%14.4lf%13.4lf%13.4lf%13.4lf%13.5lf%13.5lf%13.5lf   \n",
			week, imr_data._imu_raw_data._time_sow, Rad2Deg(ins_res_now.position.B), Rad2Deg(ins_res_now.position.L), ins_res_now.position.H,
			ins_res_now.velocity.VN, ins_res_now.velocity.VE, ins_res_now.velocity.VD,
			Rad2Deg(ins_res_now.attitude.E.Roll), Rad2Deg(ins_res_now.attitude.E.Pitch), Rad2Deg(ins_res_now.attitude.E.Yaw));
		outresfile << buffer;
		//当后续不再有GNSS观测值时，结束解算，不再输出
		if (epoch_index > _gnss._data_size)
		{
			break;
		}
	}
	fclose(imr_fp);
	outresfile.close();
	//outbiasfile.close();
	fprintf(stdout, "NavNewton NORMAL END\n");
	fflush(stdout);
    return 0;

}


